//Configure gain here
//Higher numbers = less sencitive
// 0x00 through 0x0f
int redGain = 0x02;
int greenGain = 0x02;
int blueGain = 0x02;
int clearGain = 0x02;

int ledPin = 5;

//Include the I2C Arduino library
#include <Wire.h>

//7 bit I2C address of this sensor
#define I2C_ADDRESS       0x74

#define REG_CAP_RED       0x06
#define REG_CAP_GREEN	  0x07
#define REG_CAP_BLUE	  0x08
#define REG_CAP_CLEAR	  0x09

#define REG_INT_RED_LO	  0x0A
#define REG_INT_RED_HI	  0x0B
#define REG_INT_GREEN_LO  0x0C
#define REG_INT_GREEN_HI  0x0D
#define REG_INT_BLUE_LO	  0x0E
#define REG_INT_BLUE_HI	  0x0F
#define REG_INT_CLEAR_LO  0x10
#define REG_INT_CLEAR_HI  0x11

#define REG_DATA_RED_LO	  0x40
#define REG_DATA_RED_HI	  0x41
#define REG_DATA_GREEN_LO 0x42
#define REG_DATA_GREEN_HI 0x43
#define REG_DATA_BLUE_LO  0x44
#define REG_DATA_BLUE_HI  0x45
#define REG_DATA_CLEAR_LO 0x46
#define REG_DATA_CLEAR_HI 0x47

//initial darkLevel;
int calibrationDarkness = 0;
byte calibrationRed = 5;
byte calibrationGreen = 5;
byte calibrationBlue = 5;

void setup(void){
	Serial.begin(9600);
	Wire.begin();

        pinMode(ledPin, OUTPUT);
        analogWrite(ledPin, 96);

	// sensor gain setting (Avago app note 5330)
	// CAPs are 4bit (higher value will result in lower output)
	set_register(REG_CAP_RED, redGain);
	set_register(REG_CAP_GREEN, greenGain);
	set_register(REG_CAP_BLUE, blueGain);
	set_register(REG_CAP_CLEAR, clearGain);

	int ledGain = getColorGain();

	set_gain(REG_INT_RED_LO,ledGain);
	set_gain(REG_INT_GREEN_LO,ledGain);
	set_gain(REG_INT_BLUE_LO,ledGain);
}

void loop() { 

	int clearGain = getClearGain();
	set_gain(REG_INT_CLEAR_LO,clearGain);
	int colorGain = getColorGain();
	set_gain(REG_INT_RED_LO,colorGain);
	set_gain(REG_INT_GREEN_LO,colorGain);
	set_gain(REG_INT_BLUE_LO,colorGain);

	//reset the RGB (and clear) values
	int cc = 0;
	int red=0;
	int green=0;
	int blue=0;

	// Take 4 samples, and add them together.
	for (int i=0; i<4 ;i ++) {
		performMeasurement();
		cc +=get_readout(REG_DATA_CLEAR_LO);
		red +=get_readout(REG_DATA_RED_LO);
		green +=get_readout(REG_DATA_GREEN_LO);
		blue +=get_readout(REG_DATA_BLUE_LO);
	}

	//now, divide the totals for each by 4 to get their average.
	cc/=4;
	red /=4;
	green /=4;
	blue /=4;

	//take the values mesured from above, and multiply them with the factors to
	//find out what value should be sent to the external RGB LED to reproduce this color
	float redValue = (float)red*redFactor;
	float greenValue = (float)green*greenFactor;
	float blueValue = (float)blue*blueFactor;

	Serial.print("red: ");
	Serial.print(redValue);

	Serial.print(". green: ");
	Serial.print(greenValue);

	Serial.print(". blue: ");
	Serial.println(blueValue);

	//hold it for one second
	delay(3000);
}

int getClearGain() {
	int gainFound = 0;
	int upperBox=4096;
	int lowerBox = 0;
	int half;

	while (!gainFound) {
		half = ((upperBox-lowerBox)/2)+lowerBox;

		if (half == lowerBox) { //no further halfing possbile
			break; //no further halfing possbile
		}
		else {
			set_gain(REG_INT_CLEAR_LO,half);
			performMeasurement();
			int halfValue = get_readout(REG_DATA_CLEAR_LO);

			if (halfValue > 1000) {
				upperBox=half;
			}
			else if (halfValue<1000) {
				lowerBox = half;
			}
			else {
				break; //no further halfing possbile
			}
		}
	}
	return half;
}

int getColorGain() {
	int gainFound = 0;
	int upperBox=4096;
	int lowerBox = 0;
	int half;
	while (!gainFound) {
		half = ((upperBox-lowerBox)/2)+lowerBox;

		if (half==lowerBox) { //no further halfing possbile
			break; // gain found
		}
		else {

			set_gain(REG_INT_RED_LO,half);
			set_gain(REG_INT_GREEN_LO,half);
			set_gain(REG_INT_BLUE_LO,half);
			performMeasurement();
			int halfValue = 0;

			halfValue=max(halfValue,get_readout(REG_DATA_RED_LO));
			halfValue=max(halfValue,get_readout(REG_DATA_GREEN_LO));
			halfValue=max(halfValue,get_readout(REG_DATA_BLUE_LO));

			if (halfValue>1000) {
				upperBox=half;

			}
			else if (halfValue<1000) {
				lowerBox=half;

			}
			else {
				break; // gain found
			}
		}
	}
	return half;
}

void performMeasurement() {
	set_register(0x00,0x01); // start sensing

	while(read_register(0x00) != 0) {
		// waiting for a result
	}
}

int get_readout(int readRegister) {
	return read_register(readRegister) + (read_register(readRegister+1)<<8);
}

void set_gain(int gainRegister, int gain) {
	if (gain <4096) {
		uint8_t hi = gain >> 8;
		uint8_t lo = gain;

		set_register(gainRegister, lo);
		set_register(gainRegister+1, hi);
	}
}

void set_register(unsigned char r, unsigned char v){
	Wire.beginTransmission(I2C_ADDRESS);
	Wire.write(r);
	Wire.write(v);
	Wire.endTransmission();
}

unsigned char read_register(unsigned char r){
	unsigned char v;
	Wire.beginTransmission(I2C_ADDRESS);
	Wire.write(r); // register to read
	Wire.endTransmission();

	Wire.requestFrom(I2C_ADDRESS, 1); // read a byte
	while(!Wire.available()) {
		// waiting
	}
	v = Wire.read();
	return v;
}
